/**

Onestep Build:
  cl /Ox /arch:AVX2 /MD /std:c++17 /EHsc /wd4819 /wd4828 /source-charset:utf-8 /execution-charset:gbk /I./src/ ./src/compensation.cpp  /Fo:./build/msvc/release/compensation.obj
    /link /DLL /IMPLIB:./build/msvc/release/compensation.lib /MACHINE:X86 /LIBPATH:D:/opt/vcpkg/installed/x86-windows/lib /LIBPATH:"D:/opt/AGRS GeoProbe/DevelopKit/GP_Developer/Lib" /out:./build/msvc/release/compensation.dll

Seperate Build:
  cl /c /Ox /arch:AVX2 /MD /std:c++17 /EHsc /wd4819 /wd4828 /source-charset:utf-8 /execution-charset:gbk /I./src/ ./src/compensation.cpp  /Fo:./build/msvc/release/compensation.obj
  link ./build/msvc/release/compensation.obj /DLL /IMPLIB:./build/msvc/release/compensation.lib /MACHINE:X86 /LIBPATH:D:/opt/vcpkg/installed/x86-windows/lib /LIBPATH:"D:/opt/AGRS GeoProbe/DevelopKit/GP_Developer/Lib" /out:./build/msvc/release/compensation.dll

**/

#include <string>
#include <vector>
#include <cassert>
#include <cmath> // std::hypot
// #include <gsl/gsl>  // microsoft gsl

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <filesystem>
namespace fs = std::filesystem; // create_directory std::filesystem::create_directories

// KFR Butterworth Filter
#define _USING_KFR
// #define _USING_SOS
// #define _KFR_PLOT
// #include "filter_data.h"

#ifdef _USING_KFR
  #include "sos2tf_kfr.h"
#endif

#include "utils.h"
#include "matrix_utils.h"

#include "stdvec_serialization.h"
#include "compensation.h"
#include <Eigen/Dense>

// json R/W json file.
#include <iostream>   // std::ifstream, std::ofstream
#include <fstream>    // error: variable ‘std::ifstream jenv_in’ has initializer but incomplete type
#include <nlohmann/json.hpp>
using json = nlohmann::json;

// GP2 Interface
#include <IGeoProbeBase.h>
#pragma comment( lib, "IGdbms2.lib" )

// Zero-Phase Filter FILTFILT
#include <iostream>
#include <sstream>
#include <fstream>
#include "FiltFilt/FiltFilt.hpp"
using namespace kb::math;

//int WINAPI WinMain(HINSTANCE hInstance, HINSTANCE hPrevInstance,
//                   PSTR lpCmdLine, int nCmdShow)
//{
//  return 0;
//}

extern "C" __declspec(dllexport) bool gdb_compensate(
  HANDLE hGDB,                   // GDB handle          // 输入
  COMPENSASION_PARAMS* p_params  // Compensasion params // 输入
)
{
  // 创建中间文件目录 ./intermediate/
  const fs::path intermediate("intermediate");
  if (!exists(intermediate)) {
    fs::create_directory(intermediate);
    std::cout << "创建中间文件目录: " << "./intermediate" << std::endl;
  }

  // 从json文件读取补偿系数
  std::cout << std::boolalpha;
  std::cout << "json_coef_file: " << p_params->json_coef_file << std::endl;
  std::string filename = p_params->json_coef_file; //"compensason.json";
  std::ifstream json_in(filename);
  json json_env;
  json_in >> json_env;
  std::vector<double> vec_coef = json_env["coef"];
  Eigen::VectorXd eigen_coef = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(vec_coef.data(), vec_coef.size());
  bool _using_sos = json_env["_USING_SOS"];
  std::cout << "_USING_SOS: " << std::boolalpha << _using_sos << std::endl;
  // Get butterworth filter params from json
  std::vector<double> a1 = json_env["a1"];
  std::vector<double> b1 = json_env["b1"];
  std::vector<double> a2 = json_env["a2"];
  std::vector<double> b2 = json_env["b2"];
  std::vector<double> a3 = json_env["a3"];
  std::vector<double> b3 = json_env["b3"];

  std::cout << "Python filter:" << std::endl;
  std::cout << "LowPass: " << json_env["LowPass"] << std::endl;
  std::cout << "  b1: " << b1 << std::endl;
  std::cout << "  a1: " << a1 << std::endl;
  //std::cout << "BandPass: " << json_env["BandPass"] << std::endl;
  std::cout << "BandPass: " << !json_env["HighPass"] << std::endl;
  std::cout << "  b2: " << b2 << std::endl;
  std::cout << "  a2: " << a2 << std::endl;
  std::cout << "HighPass: " << json_env["HighPass"] << std::endl;
  std::cout << "  b3: " << b3 << std::endl;
  std::cout << "  a3: " << a3 << std::endl;

  // Setup Low Pass filter
  std::cout << "Setup Low Pass" << std::endl;

  const size_t order = json_env["order"];
  std::string type = json_env["type"];
  std::cout << "Filter Order: " << order << std::endl;
  std::cout << "fiter type: " << type << std::endl;

  auto rp = json_env["rp"];
  auto rs = json_env["rs"];
  std::cout << "Filter rp: " << rp << std::endl;
  std::cout << "Filter rs: " << rs << std::endl;

  auto wn1 = json_env["wn1"];
  auto wn2 = json_env["wn2"];
  auto wn3 = json_env["wn3"];

  // Setup Butterworth C++
#ifdef _USING_KFR
  KFR_FUNCTION zpk<double> kfr_proto = kfr::butterworth<fbase>(order);

  // switch (type.c_str()) {
  //   case "cheby1":
  //     kfr_proto = kfr::chebyshev1<fbase>(order, 1);
  //     break;
  //   case "cheby2":
  //     kfr_proto = kfr::chebyshev2<fbase>(order, 60);
  //     break;
  //   default:
  //     kfr_proto = kfr::butterworth<fbase>(order);
  //     break;
  // }

  if (type == "cheby1")
    kfr_proto = kfr::chebyshev1<fbase>(order, rp);
  else if (type == "cheby2")
    kfr_proto = kfr::chebyshev2<fbase>(order, rs);
  else if (type == "bessel")
    kfr_proto = kfr::bessel<fbase>(order);
  else
    kfr_proto = kfr::butterworth<fbase>(order);

  zpk<fbase> filt_lowpass  =  kfr::iir_lowpass(kfr_proto, wn1);
  std::vector<biquad_params<fbase>> bqs1 = to_sos(filt_lowpass);
  printf("lowpass:\n");
  // print_sos(bqs1);
  std::vector< std::vector<fbase> > tf1 = sos2tf(bqs1);
  std::cout << "  b1 = " << tf1[0] << std::endl;
  std::cout << "  a1 = " << tf1[1] << std::endl;
  std::cout << "  using C++ KFR filter." << std::endl;

  zpk<fbase> filt_bandpass = kfr::iir_bandpass(kfr_proto, wn2[0], wn2[1]);
  std::vector< biquad_params<fbase> > bqs2 = to_sos(filt_bandpass);
  printf("bandpass:\n");
  // print_sos(bqs2);
  std::vector< std::vector<fbase> > tf2 = sos2tf(bqs2);
  std::cout << "  b2 = " << tf2[0] << std::endl;
  std::cout << "  a2 = " << tf2[1] << std::endl;

  zpk<fbase> filt_highpass  =  kfr::iir_highpass(kfr_proto, wn3);
  std::vector<biquad_params<fbase>> bqs3 = to_sos(filt_highpass);
  printf("highpass:\n");
  // print_sos(bqs3);
  std::vector< std::vector<fbase> > tf3 = sos2tf(bqs3);
  std::cout << "  b3 = " << tf3[0] << std::endl;
  std::cout << "  a3 = " << tf3[1] << std::endl;

  b1 = tf1[0];
  a1 = tf1[1];
  b2 = tf2[0];
  a2 = tf2[1];
  b3 = tf3[0];
  a3 = tf3[1];  
#endif // #if _USING_KFR

  // Setup Low Pass filter
  FilterCoefficients<double> lowpass_coefs;
  // lowpass_ss >> lowpass_coefs;
  lowpass_coefs.m_CoefficientsA = a1; //{1.,-1.56101808,0.64135154};
  lowpass_coefs.m_CoefficientsB = b1; //{0.02008337,0.04016673,0.02008337};
  //std::cout << "lowpass_ss: " << lowpass_ss << std::endl;
  FiltFilt<double> filtfilt_lowpass(lowpass_coefs);

  // Setup Band Pass filter
  std::cout << "Setup Band Pass" << std::endl;
  FilterCoefficients<double> bandpass_coefs;
  bandpass_coefs.m_CoefficientsA = a2; //{1.,-3.66102926,5.09866292,-3.20227714,0.7660066};
  bandpass_coefs.m_CoefficientsB = b2; //{0.00782021,0.,-0.01564042,0.,0.00782021};
  FiltFilt<double> filtfilt_bandpass(bandpass_coefs);

  // Setup High Pass filter
  std::cout << "Setup High Pass" << std::endl;
  FilterCoefficients<double> highpass_coefs;
  highpass_coefs.m_CoefficientsA = a3;
  highpass_coefs.m_CoefficientsB = b3;
  FiltFilt<double> filtfilt_highpass(highpass_coefs);

  // 初始化补偿4要素
  //printf("Init Field Attr\n");
  const char* in_name[4]; // = {"姿态X分量", "姿态Y分量", "姿态Z分量", "未补偿磁总场"};
  in_name[0] = p_params->fc_X;
  in_name[1] = p_params->fc_Y;
  in_name[2] = p_params->fc_Z;
  in_name[3] = p_params->mag_uncompensated;
  FieldAttribute in_attr[4];

  for (int j = 0; j < 4; j++) {
  GetFieldAttributeByName(hGDB, (char*)in_name[j], in_attr[j]);
  }

  std::cout << std::endl;  // EliteIV not found!
  // 创建信息组
  const char* out_name[5]; // = {"未补磁低通2", "未补磁高通2", "补偿值2", "已补磁高通2", "已补磁总场2"};
  out_name[0] = p_params->mag_uncompensated_lp;
  out_name[1] = p_params->mag_uncompensated_hp;
  out_name[2] = p_params->mag_compensation;
  out_name[3] = p_params->mag_compensated_hp;
  out_name[4] = p_params->mag_compensated_total;
  FieldAttribute out_attr[5];

  // for (int j = 0; j < 4; j++) {
  //   GetFieldAttributeByName(hGDB, (char *)out_name[j], out_attr[j]);
  // }
  for (int j = 0; j < 5; j++) {
  memcpy(&out_attr[j], &in_attr[3], sizeof(FieldAttribute));
    strcpy(out_attr[j].Name, out_name[j]);
    out_attr[j].Access = Field_ReadWrite;
    SetGDBField(hGDB, out_attr[j].Name);
  }

  Eigen::MatrixXd mat_in, mat1, mat17, mat_out;
  std::vector<double> stdvec;
  char sLine[16]; // = {"RESET"};
  strcpy_s(sLine, 16, "RESET");
  std::cout << std::endl;

  for (int ii = 0; 1; ii++) {
  LineIndex2 LineInd;
  RLineInd2(hGDB, sLine, LineInd);

    if (!strcmp(LineInd.Line, "DUMMY")) {
      break;  // 顺序读完数据了！
    }

    ////////////////////
    // Beigin Process //
    ////////////////////
    int M = (LineInd.Fid2 - LineInd.Fid1 + 1) * in_attr[3].Hits;
    printf("序号 %2d 线号(%s): [Fid1, Fid2]=[%6d, %6d], M = %d\n", ii, (char*)LineInd.Line, LineInd.Fid1, LineInd.Fid2, M);
    mat_in.resize(M, 4);

    // 读取N信息组的数据！
    for (int j = 0; j < mat_in.cols(); j++) {
      double* line = mat_in.col(j).data();
      RFieldEx(hGDB, in_attr[j], LineInd.Fid1, LineInd.Fid2, M * sizeof(double), line);
    }

    Save_Matrix("./intermediate/mat_in.txt", mat_in);
    std::vector<double> out(M);
    const std::string options = "phaseresp=True, log_freq=True, freq_dB_lim=(-160, 10)";

    // step 1: perform low pass filter
    if (json_env["LowPass"]) {
      std::cout << "Perform lowpass to 4 terms" << std::endl;

      for (int j = 0; j < mat_in.cols(); j++) {
        double* line = mat_in.col(j).data();

        if (!json_env["_USING_SOS"]) {
          std::vector<double> stdvec(line, line + M);
          std::vector<double> out = filtfilt_lowpass.ZeroPhaseFiltering(stdvec);
#ifdef _KFR_PLOT

          if (j == mat_in.cols() - 1) {
            plot_save("biquad_lowpass", out, options + ", title='Biquad low pass (0.1)'");
          }

#endif
          mat_in.col(j) = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(out.data(), out.size());
        } else {
          //constexpr size_t maxorder = order;
          kfr::univector<fbase> univec = kfr::make_univector(line, M);
          kfr::univector<fbase> out = kfr::biquad<32>(bqs1, univec);
#ifdef _KFR_PLOT

          if (j == mat_in.cols() - 1) {
            plot_save("biquad_lowpass", out, options + ", title='Biquad low pass (0.1)'");
          }

#endif
          mat_in.col(j) = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(out.data(), out.size());
          // for (int i=0; i<M; i++) line[i] = out[i];
        }

        //mat_in.col(j) = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(out.data(), out.size());
      }

      Save_Matrix("./intermediate/mat_in(lowpass).txt", mat_in);
    } else {
      std::cout << "Skip lowpass to 4 terms" << std::endl;
    }

    // step 2: calc derivatives of cosX, cosY, cosZ
    mat17.resize(M, 17);
    Eigen::VectorXd sqt_norm(M);

    for (int i = 0; i < M; i++) {
      sqt_norm[i] = std::hypot(mat_in(i, 0), mat_in(i, 1), mat_in(i, 2));
    }

    for (int j = 0; j < 3; j++) {
      for (int i = 0; i < M; i++) {
        mat_in(i, j) /= sqt_norm[i];
      }
    }

    Save_Matrix("./intermediate/mat_in(normalized).txt", mat_in);
    // temp matrix store gradients.
    mat1.resize(M, 3);
    //mat1 = mat_in.block(0, 0, M, 3);

    // calc grad of edges
    for (int j = 0; j < 3; j++) {
      mat1(0, j) = (mat_in(1, j) - mat_in(0, j)) / 0.1, mat1(M - 1, j) = (mat_in(M - 1, j) - mat_in(M - 2, j)) / 0.1;
    }

    // calc center grad
    for (int j = 0; j < 3; j++) {
      for (int i = 1; i < M - 2; i++) {
        mat1(i, j) = (mat_in(i + 1, j) - mat_in(i - 1, j)) / (2 * 0.1);
      }
    }

    Save_Matrix("./intermediate/mat1(grad).txt", mat1);

    // step 3: construct 16 terms of X and y (M rows, 16 N)
    for (int i = 0; i < M; i++) {
      // 16 N (3 N): cosX, cosY, cosZ;
      // 16 N (5 N): cosX*cosX, cosX*cosY, cosX*cosZ, cosY*cosY, cosY*cosZ
      // 16 N (8 N): cosX*dcosX, cosX*dcosY, cosX*dcosZ,
      //             cosY*dcosX, cosY*dcosY, cosY*dcosZ,
      //             cosZ*dcosX, cosZ*dcosY
      mat17(i, 0)  = mat_in(i, 0);
      mat17(i, 1)  = mat_in(i, 1);
      mat17(i, 2)  = mat_in(i, 2);
      mat17(i, 3)  = mat_in(i, 0) * mat_in(i, 0);
      mat17(i, 4)  = mat_in(i, 0) * mat_in(i, 1);
      mat17(i, 5)  = mat_in(i, 0) * mat_in(i, 2);
      mat17(i, 6)  = mat_in(i, 1) * mat_in(i, 1);
      mat17(i, 7)  = mat_in(i, 1) * mat_in(i, 2);
      mat17(i, 8)  = mat_in(i, 0) * mat1(i, 0);
      mat17(i, 9)  = mat_in(i, 0) * mat1(i, 1);
      mat17(i, 10)  = mat_in(i, 0) * mat1(i, 2);
      mat17(i, 11)  = mat_in(i, 1) * mat1(i, 0);
      mat17(i, 12)  = mat_in(i, 1) * mat1(i, 1);
      mat17(i, 13)  = mat_in(i, 1) * mat1(i, 2);
      mat17(i, 14)  = mat_in(i, 2) * mat1(i, 0);
      mat17(i, 15)  = mat_in(i, 2) * mat1(i, 1);
    }

    mat17.rightCols<1>() = mat_in.rightCols<1>();
    Save_Matrix("./intermediate/mat17.txt", mat17);
    // step 4: perform band pass filter
    if (json_env["HighPass"]) 
      std::cout << "Perform highpass to 16+1 terms." << std::endl;
    else
      std::cout << "Perform bandpass to 16+1 terms." << std::endl;

    for (int j = 0; j < mat17.cols(); j++) {
      double* line = mat17.col(j).data();
      std::vector<double> out;
      if (!json_env["_USING_SOS"]) {
        std::vector<double> stdvec(line, line + M);
        if (json_env["HighPass"]) 
          out = filtfilt_highpass.ZeroPhaseFiltering(stdvec);
        else // BandPass
          out = filtfilt_bandpass.ZeroPhaseFiltering(stdvec);
#ifdef _KFR_PLOT
        if (j == mat_in.cols() - 4) {
          plot_save("biquad_bandpass", out, options + ", title='Biquad band pass (0.04,0.1)'");
        }
#endif
        mat17.col(j) = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(out.data(), out.size());
      } else {
        //constexpr size_t maxorder = order;
        kfr::univector<fbase> univec = kfr::make_univector(line, M);
        kfr::univector<fbase> out = kfr::biquad<32>(bqs2, univec);
#ifdef _KFR_PLOT

        if (j == mat_in.cols() - 4) {
          plot_save("biquad_bandpass", out, options + ", title='Biquad band pass (0.04,0.1)'");
        }

#endif
        mat17.col(j) = Eigen::Map<Eigen::VectorXd, Eigen::Unaligned>(out.data(), out.size());
        // for (int i=0; i<M; i++) line[i] = out[i];
      }
    }

    Save_Matrix("./intermediate/mat17(bandpass)_out.txt", mat17);
    //std::cout << " End of bandpass to 16 terms." << std::endl;
    mat_out.resize(M, 5);
    // 未补磁低通
    mat_out.col(0) = mat_in.col(3);
    // 未补偿高通
    mat_out.col(1) = mat17.col(16);
    // step 5: Calc compensation via coef from json file.
    // https://eigen.tuxfamily.org/dox/group__QuickRefPage.html#title8
    //Eigen::VectorXd compensation = mat17.leftCols(16)*eigen_coef; // unoptimized
    Eigen::VectorXd compensation = mat17.leftCols<16>() * eigen_coef; // optimized
    //Save_Matrix("./intermediate/compensation_out.txt", compensation);
    mat_out.col(2) = compensation;
    // step 6: Perform compensasion
    // 已补偿高通 = 未补偿高通 - 补偿值
    mat_out.col(3) = mat_out.col(1) - compensation;
    // 已补偿总场 = 未补磁低通 - 补偿值
    mat_out.col(4) = mat_out.col(0) - compensation;
    Save_Matrix("./intermediate/mat_out.txt", mat_out);

    // step 7: write 5 columns to GDB compensated
    //if (strcmp(LineInd.Line, "L2110"))
    // printf("Write to GDB.\n");
    for (int j = 0; j < 5; j++) {
      double* line = mat_out.col(j).data();
      WFieldEx(hGDB, out_attr[j], LineInd.Fid1, LineInd.Fid2, M * sizeof(double), line);
    }

    ////////////////////
    // End of Process //
    ////////////////////
    strcpy_s((char*)sLine, 16, "NEXTIND");
    // 结束处理一条剖面数据
  }

  return true;
}
